#pragma once

#include "unitree_legged_sdk/joystick.h"
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "stair_ctrl/JoyStick.h"


using namespace UNITREE_LEGGED_SDK;

class joyStickAdvanced
{
public:
    stair_ctrl::JoyStick joy_stick;
    stair_ctrl::JoyStick joy_stick_prev;
    stair_ctrl::JoyStick joy_stick_current;

    joyStickAdvanced()
    {
        joy_stick = stair_ctrl::JoyStick();
        joy_stick_prev = stair_ctrl::JoyStick();
        joy_stick_current = stair_ctrl::JoyStick();
    }

    void getJoyStickRosMsg(xRockerBtnDataStruct &keyData_, stair_ctrl::JoyStick &joy_stick_)
    {
        joy_stick_.head[0] = keyData_.head[0];
        joy_stick_.head[1] = keyData_.head[1];

        joy_stick_.lx = keyData_.lx;
        joy_stick_.rx = keyData_.rx;
        joy_stick_.ly = keyData_.ly;
        joy_stick_.ry = keyData_.ry;
        joy_stick_.L2 = keyData_.L2;

        for (int i = 0; i < 16; ++i)
        {
            joy_stick_.idle[i] = keyData_.idle[i];
        }

        joy_stick_.keys.R1 = keyData_.btn.components.R1;
        joy_stick_.keys.L1 = keyData_.btn.components.L1;
        joy_stick_.keys.start = keyData_.btn.components.start;
        joy_stick_.keys.select = keyData_.btn.components.select;
        joy_stick_.keys.R2 = keyData_.btn.components.R2;
        joy_stick_.keys.L2 = keyData_.btn.components.L2;
        joy_stick_.keys.F1 = keyData_.btn.components.F1;
        joy_stick_.keys.F2 = keyData_.btn.components.F2;
        joy_stick_.keys.A = keyData_.btn.components.A;
        joy_stick_.keys.B = keyData_.btn.components.B;
        joy_stick_.keys.X = keyData_.btn.components.X;
        joy_stick_.keys.Y = keyData_.btn.components.Y;
        joy_stick_.keys.up = keyData_.btn.components.up;
        joy_stick_.keys.right = keyData_.btn.components.right;
        joy_stick_.keys.down = keyData_.btn.components.down;
        joy_stick_.keys.left = keyData_.btn.components.left;
    }

    uint8_t findTriggerRelease(uint8_t curr, uint8_t prev)
    {
        if (curr != prev)
        {
            return curr + 2;
        }
        else
        {
            return curr;
        }
    }

    void update(xRockerBtnDataStruct &keyData_)
    {
        joy_stick_prev = joy_stick_current;
        getJoyStickRosMsg(keyData_, joy_stick_current);
        joy_stick = joy_stick_current;

        joy_stick.keys.R1 = findTriggerRelease(joy_stick.keys.R1, joy_stick_prev.keys.R1);
        joy_stick.keys.L1 = findTriggerRelease(joy_stick.keys.L1, joy_stick_prev.keys.L1);
        joy_stick.keys.start = findTriggerRelease(joy_stick.keys.start, joy_stick_prev.keys.start);
        joy_stick.keys.select = findTriggerRelease(joy_stick.keys.select, joy_stick_prev.keys.select);
        joy_stick.keys.R2 = findTriggerRelease(joy_stick.keys.R2, joy_stick_prev.keys.R2);
        joy_stick.keys.L2 = findTriggerRelease(joy_stick.keys.L2, joy_stick_prev.keys.L2);
        joy_stick.keys.F1 = findTriggerRelease(joy_stick.keys.F1, joy_stick_prev.keys.F1);
        joy_stick.keys.F2 = findTriggerRelease(joy_stick.keys.F2, joy_stick_prev.keys.F2);
        joy_stick.keys.A = findTriggerRelease(joy_stick.keys.A, joy_stick_prev.keys.A);
        joy_stick.keys.B = findTriggerRelease(joy_stick.keys.B, joy_stick_prev.keys.B);
        joy_stick.keys.X = findTriggerRelease(joy_stick.keys.X, joy_stick_prev.keys.X);
        joy_stick.keys.Y = findTriggerRelease(joy_stick.keys.Y, joy_stick_prev.keys.Y);
        joy_stick.keys.up = findTriggerRelease(joy_stick.keys.up, joy_stick_prev.keys.up);
        joy_stick.keys.right = findTriggerRelease(joy_stick.keys.right, joy_stick_prev.keys.right);
        joy_stick.keys.down = findTriggerRelease(joy_stick.keys.down, joy_stick_prev.keys.down);
        joy_stick.keys.left = findTriggerRelease(joy_stick.keys.left, joy_stick_prev.keys.left);
    }
};